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ABSTRACT 



A discrete state space model is developed which describes an autonomous under- 
water vehicle and incorporates the elTects of currents, sea state, and wind as it travels 
through the sea. Heading commands are calculated to overcome these effects by the 
design of an Extended Kalman Filter which estimates their combined velocity compo- 
nents. Simulations are done which test the filter's effectiveness in a range of different 
environments. Some potential uses for this system are discussed at the end. 
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I. INTRODUCTION 



A. BACKGROUND 

With the increased costs for Manned Naval Suface and Subsurface assets and the 
greatly increased capability of small computing devices, using small autonomous under- 
water vehicles (AL’Vs) to carryout certain missions has become increasingly desirable. 
A vehicle's capability to navigate to a precise location independently and in many cases 
covertly is essential. 

Current research into potential navigation systems that measure a vehicle's position 
and unknown forces acting on it include sonar doppler, stapdown Inertial Measuring 
Units (IMUs), and sonar triangularization. The two primary' IMUs underdevelopment 
for AUVs are the Laser Ring Gyro and the Fiber Optic Gyro (FOG). The systems 
mentioned above each have their own independent drift characteristics which have to 
be filtered out and their actual positions updated from time to time using an outside 
source such as the Navstar Global Positioning System (GPS) or Omega. In addition, 
they have large power requirements, are bulky (with the possible exception of the FOG), 
and require compex control circuitry. If a microprocessor could be programmed to es- 
timate the underwater forces acting on the vehicle enroute to the destination, based on 
updates by GPS, and then to apply a heading command to compensate for any devi- 
ations in its course, then the power requirements, complexity, and cost could be 
signifcantly reduced. Figure 1 on page 2 illustrates a block diagram of a possible sys- 
tem. 

In this Thesis, a state space model will be developed based on a generic vehicle's 
dynamics. The forces acting on the vehicle will be modeled as a velocity vector with an 
angle (Drift) and a magnitude (Set) with the following assumptions: 

• the angle and magnitude of this vector are white random Gaussian variables with 
zero mean about predicted values, and 

• the overall Set and Drift between sample intervals remains a constant. 

In order to estimate this set and drift, an extended Kalman Filter is developed. 
Various simulations are performed which explore the behavior of the filter and its ability 
to estimate the velocity components. The simulation was done using DSL (Dynamic 
Simulation Language) on the Naval Postgraduate School's IBM 3033 Mainframe Com- 
puter. 
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DESTINATION 



Figure 1. System Block Diagram 




B. GLOBAL POSITIONING SYSTEM 



The Navstar Global Positioning System is a space-based radio navigation system 
which operates passively. GPS is a tri-service multiagency program managed by the Air 
Force's GPS Joint Program Office. When fully operational in the early 1990's, the sys- 
tem will consist of 18 satellites with three orbiting spares, in 6 orbital planes. Each sat- 
ellite transmits two unique codes, the clear acquision (C/A) code and the Precise (P) 
code. These codes, which are unique to each satellite, are modulated and two L Band 
frequencies, LI and L2, as pseudo random noise sources along with the Navigation 
Message. LI contains both the C/A and P codes while L2 has just the P code. 

The user's receiver generates the identical codes and correlates its code with the 
satellite's. The amount of slewing theat the receiver must do in order to obtain a match 
is the time that the two signals are off. Dividing by the speed of light and correcting for 
atmospheric delays and any clock differences produces the range to the satellite. Ob- 
taining three ranges from three different satellites is required to obtain a 3 dimensional 
coordinate position fix. However, in order to accomplish this, the receiver's clock would 
have to be identical in time to that of the GPS's Master Clock which would defeat the 
purpose of the system. To provide this time differential, the receiver must lock onto 4 
satellites and solve four simultaneous equations for the three coordinates and the time 
differential in order to get the fix. 

The GPS Navigation Message supplies the user with the satellite's clock offset, at- 
mospheric delays, and satellite ephemeris in order to accurately solve the four equations. 
It is modulated on both LI and L2 at a 50 bit per second rate and is contained in a 1500 
bit frame. This frame is divided into 5 subframes which contain specific blocks of data. 
This format is shown in Figure 2 on page 4. 

The first subframe contains the clock correction parameters and ionospheric propa- 
gation delay model parameters. The second and third subframes contain the satellite's 
ephemeris which is updated every 1-5 hours by the ground control segment of GPS. 
Subframe four is for any special messages which must be passed onto the users. 

The final subframe contains the almanac data. The almanac data is an abbreviated 
form of the first three subframes for each of the satellites. This data is transmitted on 
a rotating basis and requires 19 subframes to transmit it all and is used only to help lo- 
cate the four satellites which are overhead. The almanac must be loaded into the re- 
ceiver either manually or by the receiver itself by locking onto a satellite and obtaining 
the block five message through an entire cycle. 
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Figure 2. Navigation Message Contents 
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C. GPS RECEIVER 



In order to perform the tasks that will be referred to later in this thesis, the GPS 
receiver must be capable of accomplishing the following tasks: 

• Obtain current position 

• Store the necessary waypoints, including the start and end points. 

• Provide course information to next waypoint. 

• Have the ability to survive temperature, pressure, and other conditions associated 
with the marine environment. 

Currently under development for the Marine Corps by the Naval Ocean Systems 
Command (NOSC) San Diego is a receiver called the Small Unit Navigation System 
(SUNS). [Ref.l] This receiver is designed to be handheld, rugged, lightweight, and bat- 
tery operated for the Marine Corp's Reconnaissance teams during long range operations 
on both land and underwater. SUNS will be able to store up to waypoints, including 
the destination, and will provide the user with current position, velocity, cross track er- 
ror, distance and bearing to next waypoint, and time with a positional accuracy of 50 ft. 

One additional requirement which should be considered is equipping the receiver 
with a multi-channel capability r to lock on all four satellites at the same time. This would 
allow the vehicle to obtain a position fix quicker and thus spend less time on or near the 
surface. The SUN'S receiver is being designed as a single channel unit and will have to 
sequentially correlate each of the signals from the four satellites. 
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II. SYSTEM MODEL 



A. DYNAMIC MODEL 

An underwater vehicle will travel through the water with a desired heading and at 
an ordered speed. The combined external forces acting on the vehicle will cause it to 
deviate from the intended track at some angle (Drift) and velocity (Set). This deviation 
can be modeled as a single velocity vector when added to the vehicle's own velocity 
vector results in the True Velocity vector along which the vehicle will travel. This re- 
lationship is shown in Figure 3 on page 7, where a is the vehicle's commanded heading, 
y is the angle for the True Velocity, and ^ is the angle of the drift's velocity. The simple 
addition of the indvidual components produce Equations 2.1 and 2.2 for the x and y 
components of Vt, x r and yy. 



x T = Vel cos (a) + Set cos( >/0 


(2.1) 


y T = Vel sin(a) + Set sin(t^) 


(2.2) 



Developed in Reference 2, the vehicle's dynamic model, neglecting any sway dy- 
namics, is illustrated in Figure 4 on page 8. In this model, a heading input (Ildg) 
produces a heading direction ( a ), whose sine and cosine are multiplied by the vehicle's 
ordered speed (Vel) to produce the ordered velocity components (.vandy). If these 
components are summed with their respective drift components (ux and tty), then the 
components of the true velocity (,r t and y,) are obtained. 

G(s) is the transfer function describing the vehicle's true dynamics, which for the 
generic vehicle used in this thesis will be a simplified first order system. With k = 4.4 arid 

G(s) = — — - , a transient response as shown in Figure 5 on pace 9 is realized 
s 4" 4 w 

I1DGCOM is the applied heading command and ALPHA is the resultant vehicle head- 
ing angle ( a ). This G(s) was chosen as a reduced order model based on various vehicles 
which are now in operation. K = 4.4 produces a 2 sec. settling time with little overshoot. 
This type of response will minimize the effect of the vehicle's response to heading com- 
mands on the Kalman Estimator updating during the simulations in Chapter 4 and is 
close to what a real vehicle would produce. 

As can be seen from the model, this is an open loop system where the heading angle 
is put in from an outside source and maintained by the position feedback of a . This 
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Figure 3. Velocity Components Encountered by the Vehicle 
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A 



A 




Figure 4. Dynamic Model [ Adapted from Ref. 2 : p 19 ] 
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Figure 5. 



Step Response for Vehicle 
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heading command will be provided by the processor after calculating ux and uy and de- 
termining an a which will negate these inputs. 

B. STATE SPACE REPRESENTATION 

In this section a discrete state space model is developed based on the dynamic model 
above along with the identification of the individual states. 

The linear discrete state space model is described by Equations 2.3 and 2.4 [Ref.3: 
pp. 66-67] where x k is the current value of the states at time = t and x k ^ contains the 
predicted values of the states at time = 1 4- T. T is defined as the sampling period. The 
definitions for the variables in the equations below are summarized in Figure 6. 

Af+i = < ^x k + r l u k + r 2 w k (2.3) 

4 = Il£k + ft (2.4) 

x = .V x 1 state vector 

u k = M x 1 vector of applied forcing functions 
w k = K x 1 vector of random forcing functions 
z = L x 1 observation vector 
v=Lx 1 measurement noise vector 
(D = .V x iV Transition matrix 
I - ! =.Vx .V/ matrix 
r 2 = .V x K matrix 
H= Lx .V matrix 



figure 6. Symbol Identification 
The following states were defined and placed into x: 



*(1) = ' Y r 


(2.5a) 


x(2)=x r 


(2.5/>) 


-v(3) = y, 


(2.5c) 


*(4) =y, 


(2.5J) 
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.v(5) = u'x 



x(6) = try 



The <T Matrix becomes 

1 7 0 0 0 0 

0 1 0 0 0 0 

0 0 1 7 0 0 

0 > = 

0 0 0 1 0 0 

0 0 0 0 1 0 

0 0 0 0 0 1 



(2.5e) 

(2.5/) 



(2.6) 



The command vector, u k , will consist of the velocity components, nx k and uy k , of any 
new heading given to the vehicle which are needed to compensate for the estimated set 
and drift. They are given in the equations below with Hdg k and Hdg k _ , as the heading 

commands at time= kT and (k-l)T respectively. 

/ 

ux k = Vel x ( cos {Hdg k ) - cos (Hdg k _ x )) (2.7) 

uy k = Vet x ( sin {Hdg k ) — sin(/7/g^_,)) (2.8) 



f, now becomes: 



r,= 



7 0 
1 0 
O' 7 
0 1 
0 0 
0 0 



(2.9) 



The x and y components of the Set and Drift are contained in w* as uw and vvv. 
Since the Set's magnitude and the Drift's direction are assumed to be white Gaussian 
random variables with zero mean about predicted values, vv.v and ivy will also be white 
Gaussian random variables with zero mean about their predicted values. This leads to 
f 2 below. 
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T 0 



1 0 

0 T 

r 2 = (2.io) 

2 0 1 

1 0 

0 1 

At this point, the observation vector, z, consists only of the position coordinates 
(.v and y) which come from GPS. The velocity components received from GPS are those 
components that the vehicle is experiencing at that instant in time and may or may not 
be the time averaged velocities which are defined in the model. If only the position co- 
ordinates are used then the system is not observable [Ref. 3: pr 67-70]. As a result an- 
other variable must be measured. 

In the system model ( Figure 4 on page 8 ), recall that a is fedback to the input in 
order to maintain the applied heading command. Since this value must be measured, it 
can serve as the other variable that will link the states. Thus the observation vector 
becomes: 



y t 



+ V 



a 



( 2 . 11 ) 



It can be observed that z is now nonlinear because a is not a linear function of the 
states .t(2), x(4), ,r(5). and .v(6). (Eq. 2.12] As a result, it cannot be treated as the linear 
function as described in Equation 2.4. 



a = arcuin 



,v( 4) — -t( 6) - 
„\-(2) - ,y(5) 



( 2 . 12 ) 



The next step is to create an estimator that will measure ux and uv in the presence 
of noise (uncertainty) in the measurements and overcome nonlinear problem of the ob- 
servation vector. In Chapter 3, this is accomplished by the design of an extended 
Kalman Filter. 
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III. STATE ESTIMATION 



A. INTRODUCTION 

In this chapter, the theory of statistical parameter estimation is applied to deter- 
mining the values of the Set and Drift components, ux and uy, which were discussed in 
Chapter 2. Because of the nonlinear measurement vector and the uncertainty generated 
in the measurement noise, a simple observer, i.e., the Luenberger Observer, is not feasi- 
ble. Due to the successful applications of the Extended Kalman Filter [Ref. 4: p. 23] in 
solving nonlinear estimation problems and the advancements in microprocessor design 
in obtaining real time solutions, the Extended Kalman Filter was chosen as the estimator 
for this problem. 

B. KALMAN FILTER 

1. The Linear Kalman Filter 

The Kalman Filter is an algorithm for extracting or estimating information from 
noisy data. More specifically, it is a parametric estimation process based on an 
autoregresive (AR) model of the plant. The filter uses samples or measurements of the 
plant as observables to recursively update estimates of the plant's state. 

When both the system and measurement models are linear functions of the 
states. The state update equation is shown below in Equations 3.1 and 3.2. In these 
equations G is the vector containing the optimum estimation gains (Kalman gains), 
is the predicted value of the states at time = k T based on the measurements ob- 
tained and any known forcing functions at time = (k — 1)7", and x k is the updated state 
estimation vector based on the new measurement vector (z k ). 



The following assumptions are made in using the Kalman equations: 

• The random forcing function (iv*) is zero mean and uncorrelated with covariance 

Q\. 

E[_w^\ = 0, for all k > 0 ,, ,, 



• The measurement noise (v k ) is zero mean and uncorrelated with covariance R k . 



x kk = x kk _ l + G(z k -Ilt kk _,) 



(3.1) 
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£[i;.] = 0. for all k > 0 



EtZky] = 



RkkPkk for a11 k =./] 
0 lor all k^j j 



• The random forcing function and measurement noise are uncorrellated. 

E[_w k vJ~] = E\_v k wJ ] = 0 for all k.j r 0 

• The random forcing function and initial states are uncorrellated. 

= 0 for all k # 0 

• The measurement noise and initial states are uncorrellated. 

t-i r -r t- 



Elr k 4 ] = ZfQo^] = 0 for all k = 0 



(3.3) 

(3.4) 

(3.5) 

(3.6) 



' he optimal estimation gains are those which satisfy the following relationships: 



k k ~ P k k~\ 'k:k-\Hk + 

?k'k = ^ f — G k Hk - I Pk\k-\ 



^ + n=<i>^*® r +r : GT 2 r 



-1 



(3.7) 

(3.8) 

(3.9) 



where P k k is the error covariance matrix update at time = k T and P MA is the predicted 
error covariance at time = (k + 1)T based on the data at time = k T. 

2. Extended Kalman Filter 

As previously mentioned, the measurement vector as described in Equations 
2.11 and 2.12 is a nonlinear functon of the states and as a result will be modeled as 
shown below, 



z k = h{x k ,k) + v k (3.10) 

In this model, the measurement vector. z k , is a function of the state variables 
plus the measurement noise vector, V h The adaptation of the Kalman Filter involves the 
linearizing of h(x k .k) by taking the Taylor Series expansion about the predieted value of 
the states. x kk _ u at time = k T and keeping only the first order terms. This is the Ex- 
tended Kalman Filter. The reader is referred to Gelb [Ref. 3: pp 180-225] for a detailed 
explanation of the process. In summary, a more precise approximation can be achieved 
in the optimal estimator by including higher terms of the series expansion but at the 
expense of simplicty and increasing the possibility of the recursion diverging. 

The linearized form of Equation 3.10 yields 
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(3.11) 



Z k = H k x k + v k 



14 = 



dhU k .k) 

cX k 



Zk = -ikk - 1 



(3.12) 



A summary of the definitions for the Extended Kalman Filter estimator for the 
linear state model [Eq. 2.3] and the linearized measurement model [Eq. 3.1 1] is displayed 
in Figure 7 on page 16. A block diagram that illustrates the algorithm for recursively 
computing the filter is shown in Figure S on page 17. 

C. MATRIX CALCULATIONS 
1. Determination of O' 

O' as defined in Equation 3.2 is the covariance matrix for the random forcing 
function of the set and drift velocity components. It is a measure of the uncertainty 
contained in the predictions. As stated previously, the set and drift experienced by the 
vehicle is assumed to be Gaussian with 0 mean about the predicted values and as a result 
their x, v components are independent and will be approximately Gaussian with 0 mean 
with the standard deviations equal to the maximum expected deviation in the x and y 
directions. Thus O' 



O' = 





(3.16) 



The method used as an approximation to calculate <, and a 2 . is the following: 

• Determine the maximum amount of variation which can be expected in the set and 
drift. 

• Calculate the cartesian components, x and y, from the values determined above. 
Set these values equal to 3o x/y 

• Divide the x and y components by 3, subtract them from their respective predicted 
components, and square to obtain a 2 wx and oO 

As an example, let the maximum expected deviation for the drift be ± 20° and 
the maximum expected deviation for the set be ± .5 knots. The predicted values for the 
Set and Drift are 45° and 1.0 knots. Using the above procedure. 
<, = 0.43 and <, = 0.43. 
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SYSTEM MODEL: x k+i = Ox k + T x u k + T 2 iv k 

where w k cc .Y[0,<2'] 

MEASURE MODEL: : k = h(x kJc ) + v k 

where w k oc A’[0,(2'] 

INITIAL CONDITIONS: oc MXfJ 

OTHER ASSUMPTIONS: E[_w k vf] = 0 for all kj > 0. 
GAIN EQUATION: 

o k - p tk .X^„p t . k .x + rt' 

ERROR COVARIANCE UPDATE: 

Pk.k~ G k II k ~\P k k _ i 

STATE ESTIMATION UPDATE EQUATION 

Sick = &kk - 1 + ~ h{.i k . k - ,)] 

ERROR COVARIANCE PROPAGATION EQUATION: 
P k+ vk = <S>P kk <t> T +Q 

STATE ESTIMATION PROPAGATION: 

x k+lk = ^£ kk + T x U k 

ch(x k ,k) 

DEFINITIONS: U k = - — — 

OX k = 

Q = r 2 QTl 

k(±k k-\) = K^kjc) I t_i 



Figure 7. Summar)' of Extended Kalman Equations 



(2.3) 

(3.10) 

(3.13) 

(3.4) 

(3.7) 

(3.8) 
(3.1) 

(3.9) 
(3.16) 

(3.12) 

(3.14) 

(3.15) 
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Figure 8. Kalman Filter Recurshe Loop 
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2. Determination of R 

R as defined in Equation 3.3, is the covariance matrix for the measurement 
noise vector. As in O', R is the uncertainty contained in the measuring of the three pa- 
rameters x, y. and a in the presence of noise. For this application, the noise for all three 
parameters is assumed to be uncorrellated amongst themselves and white Gaussian with 
0 mean. R becomes 



1 




R = 



0 

0 



0 

1 

rr; 

0 



0 

0 




(3.17) 



The variances, a x and o y , are the standard Uv'ations from GPS and are obtained 
by using the stated DOD value for the Circular Error of Probability (CEP) of 10 meters 
or whichever CEP figure is associated with the GPS Receiver in use and Equation 3.18 
[Ref. 5: p. 21]. The standard deviation for the specific type of compass used is repres- 
ented by g 0 . 



CEP = 0.59(<7 t + Oy) (3.18) 

3. Determination of x i+m 

as defined in Equation 3.1 is a predictor of the states at time = k + 1 based 
on the current estimation of the states at time = k As a result, kk-tk has to take into 
account any controls which are applied as a new heading command to overcome the 
estimated set and drift as well as the known plant dynamics. Thus x k . hk becomes 

= + (3.19) 

where r i; <t>, and u k were defined in Equation 2.3. 

4. Determination of H k 

LL k as defined in Equation 3.12 is the linearized observation vector and is ob- 
tained by taking the partial derivative of z with respect to v. Recalling Equation 2.11, 
the first two values of ~, x and y, are linear functions of only one state and their partial 
derivatives are easily computed in Equations 3.20 and 3.21. 
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( 2 . 11 ) 






arc tan 



■\'(4) - ,r(6) 

.y(2) - .v(5) J 



dz( 1 ) ax 



r- = 2^-=[l0000 0] 



CA CX 



(3.20) 



/3r(2) _ _£y_ _ [ 0 0 1 0 0 0] 



CX CX 



(3.21) 



To obtain the partial derivative of z(3), the following relationships are used. 
[Ref. 6: pp 328-330] 



d ardanju) j du 

d x 



1 + u' 



(3.22) 



d ll 1 — 1 d u 



d X 



r dx 



(3.23) 



achieved for 



Using Equations 3.21 and 3.22 and simplifying, the following results were 
~cZ(3) 



cx 



„ .v(4) — .v(6) 

, ,,, cardan— — — 

cz(3) -v(2) — -v(o) 



c.y( 1 ) 6\r( 1 ) 



= 0 



(3.24) 



-v(4) - .v 6) 

cardan— — — 

cz(j) -v(2) — .v(o) 



-v(2) - x(5) 



6x(2) cx(2) 



x{2) - x(5))- + (.v(4) - .y(6))“ 



(3.25) 



„ -v(4) - ,v( 6) 

. carcian—— — - 

czb) .vt 2 ) — .v(o) 



<5.y(3) <5.y(3) 



= 0 



(3.26) 



darctan 



cz(3) 

o.v(4) <?.y(4) 



.y(4) - ,y(6) 
.y(2) - x(5 ) 



x(4) - ,y(6) 



_ 1 



(x{2) - x{5)Y + (.y(4) - .y(6))‘ 



(3.27) 
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.y(5) - x( 2) 



(3.28) 



dziT) 

c.x(5) 



oarcian 



■v(4) — ,y(6) 
.y( 2) — .y(5) 



c.y(5) 



(,y(2)-.y(5)) 2 + (.v(4)-.y(6)) 2 



dz(3) 

c.y(6) 



cavctan 



.y(4) - .y(6) 
•v(2) - ,y(5) 



c.y(6) 



m - . y (4) 

W2)-.v(5)) 2 + (.r(4)-.r(6)) 2 



(3.29) 



The above equations are combined into Equation 3.30. which is the matrix ver- 
sion for H k . The final version, it should be noted, contains only three major calculations: 
the denominator term which is common, the .y(2) — .y(5) term, and the .y(4) — .y(5) term. 
The two terms. H 2S and II i6 are just the negatives of H n and H respectively. Thus this 
linearization is not as ominous as one would expect and can easily be implemented on 
a microprocessor. 



U k = 



1 0 0 0 0 0 

0 0 10 0 0 

0 H n 0 // 34 -// 32 -// 34 



(3.30) 



where 



-v(2) - ,y (J) 

(- v (2) — -v(5)) 2 + (.y(4) — .y(5)) 2 

.y(4) — ,y(6) 

(•y(2) - -v(5)) : + (.v(4) - .y(5)) 2 



(3.30a) 



(3.30 b) 



20 



IV. SIMULATION RESULTS 



A. INTRODUCTION 

In this Chapter, the Extended Kalman Filter calculated in Chapter 3 is tested by 
performing various simulations using the models developed in Chapter 2 and determin- 
ing if the filter is correctly estimating the six states. This testing was done in four steps. 
In the first step, the filter and program were checked to insure that the filter accurately 
estimated the states and that the program interacted with the filter while maintaining the 
vehicle's track with no added bias. Next, a bias was added to the predicted values of Set 
and Drift and a heading change is made to correct for the bias and allow the vehicle to 
continue to its destination. This test would show whether the filter would accept the 
heading change and maintain its lock. Third, the filter's ability to handle multiple Set 
and Drift changes is checked by randomly changing the bias every other sample. Last, 
random noise is added to the measurements being fed into the filter and the errors 
measured. 

The full simulation program which was used to run the last simulation is shown in 
Appendix A. The other simulations were run with this program with the unnecessary’ 
segments commented out. The program primarily consists of Input, Initialization, Dy- 
namic, State Estimation, and Correction Segments. In the Input, the values for the ve- 
hicle's speed, the origin and destination coordinates, the predicted values for the Set and 
Drift, and the standard deviations for the various random variables are entered. The 
units used for the distance and angle inputs are nautical miles and degrees. 

The Initialization segment the nautical miles, hours, and degree units are conveted 
to feet, seconds, and radian units of measure. In addition, equidistant waypoints and 
course and distance to destination are calcluated, biases are added to the predicted Set 
and Drift, and the Kalman Filter matricies arc initialized. 

The Dynamic segment calculates the actual x and y coordinates and the current 
heading angle which are used as the GPS and compass inputs into the filter by using the 
model of Figure 4 on page 8 created in Chapter 2. 

The State Estimation updates U k . h(£ kik _ l ), and z. and then calls the subroutine 
Kalman [Appendix B|. Kalman performs the matrix operations and returns the current 
state estimate, x k]k , the projected covariance of error, Rk+wki and the projected state esti- 
mate, x k , lk . Finally the returned estimates of ux and uy are used to calculate the new 
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heading command in the Course Correction segment which, in addition, updates * 
[Eqt. 3.16], 

For all the simulations, the vehicle's ordered speed or speed through the water is 
maintained as a constant. This assumes that the vehicle has an optimum speed which 
minimizes the amount of fuel consumed during a long distance voyage, which appears 
to be the trend in current research. The program could easily be modified for the vehicle 
to maintain a constant over the ground speed. The speed picked for all the runs was 7.5 
knots. 

The predicted Set and Drift were held as a constant for all the runs at 45 degrees and 
1.0 knots. The o wx and values were those calculated in the example on Page 15 as .66. 
For a x and g„, the values used were determined using Equation 3.18 and the DOD figure 
of 10 meters for the CEP. Thus, a x = o y = 25.4 feet. The value for o ti , chosen at random, 
was 1.5 degrees or .03 radians. A summary of the constants and their values are sum- 
marized in Table 1. 



Table 1. SUMMARY OF VALUES USED IN SIMULATION RUNS 



CONSTANT 


VALUE / UNITS 


T 


600. Seconds 


Set 


7.5 Knots 


Drift 


45 Degrees 


Orign Coordinates 


0. 0.0.0 Nautical Miles 


Destination Coordinates 


7. 0.6.0 Nautical Miles 




25.4 feet 


a , 


25.4 feet 




.03 radians 


a 

tv r 


.66 feet per second 


a 

1*/ V 


.66 feet per second 



For each simulation run. there are 7 graphs as output. The first one shows the ve- 
hicle's overall track as compared to the desired track using the waypoints as a reference. 
The remainder are plots of the current state estimates shown against the actual states 
as a function of time. The values for the actual states are graphed as solid lines, while 
the estimated state graphs use a dashed line. For convenience to the reader, the actual 



number of figures are reduced by combining two plots into each figure with the excep- 
tion of the vehicle’s overall position track. 

B. STATE ESTIMATION WITH NO BIAS 

In this simulation, the predicted Set and Drift are entered into the vehicle and it is 
instructed to maintain its course to the destination point. There is no added bias to the 
Set and Drift and the run is performed for 6000 seconds or 5 sampling periods. 

Figure 9 on page 24 shows the vehicle's actual track against the intended track. 
The waypoints are shown as a reference. As can be seen, the vehicle follows the intended 
track with no deviation which demonstrates that the program itself is functioning prop- 
erly. Figure 10 on page 25 provides the estimated and actual position state variables 
plotted both as a function of time and each other. In Figure 1 1 on page 26, the true 
velocity state variables, x and y, are again plotted as functions of time and estimated vs. 
actual. Figure 12 on page 27 plots the estimated and actual Set and Drift components, 
ux and tty in the same manner as before. These figures demonstrate that the filter is 
locking on after only one sample and maintains the lock throughout the run as expected. 
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Figure 9. Intended and Actual Vehicle Tracks With No Added Bias 



24 




o o 




( ton x 



W «■- ^ (*i cJ 

ft* ft* ^ f* r 

noTnNEX 



o iA 



* — x — r 



uh 



■J I 3 



R 



m o 



TT&u mix 



Figure 10. Position Coordinates With No Added Bias 
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Figure 11. Velocity Components With No Added Bias 
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Figure 12, Set and Drift Components With No Added Bias 
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TIME M0> 



C. STATE ESTIMATION WITH BIAS AND COURSE CORRECTION 

During this simulation, the filter's ability to maintain track during a course change 
was determined. A bias of .2 radians was added to the drift and .5 knots was added to 
the Set. After the filter determined an estimate for the Set and Drift components, the 
simulation program added them to a new desired course to arrive at the new heading 
command and updated x :i _ vll . The new desired course was calculated by comparing the 
vehicle's current position to it's destination and subtracting the respective components. 
Aswas previously done, the length of the run was 6000 seconds. The results are dis- 
played in Figure 13 on page 29. Figure 14 on page 30, Figure 15 on page 31. and 
Figure 16 on page 32. 

The figures demonstrate the program and filter's ability to correctly determine the 
overall Set and Drift and ?ive a correct heading command to guide the vehicle to its 
destination. As can be seen, the filter successfully acquires ux and uy after only one 
sample and maintains a lock there after. This allows the vehicle to stay on course 
without having to constantly make corrections due to bad estimates from the filter. 
Next, the filter's ability to handle multiple Set and Drift changes will be tested. 
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Figure 13. Intended and Actual Vehicle Tracks With Bias and Course Correction 
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Figure 14. Position Coordinates With Bias and Course Correction 
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Figure 15. Velocity Components With Bias and Course Correction 
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Figure 16. Set and Drift Components With Bias and Course Correction 
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TIME MO* 




D. STATE ESTIMATION WITH MULTIPLE SET AND DRIFT CHANGES 

In this simulation, the Set and Drift was varied in a random manner at the beginning 
of every other sample. Values were created using the Normal function in DSL and 
added to the predicted Set and Drift. The Normal function randomly generates num- 
bers that are distributed according to a Gaussian distribution function about a mean 
with a standard deviation. In this case the mean was zero and the standard deviation 
for the Drift was 7 degrees or .12 radians and .2 knots for the Set. The run was for 4200 
seconds in order to obtain multiple variations at the sampling period of 600 seconds. 
The results were plotted and are displayed in the same manner as the previous two sim- 
ulations. 

In Figure 17 on page 34, the vehicle's actual track is off the intended track by as 
much as 500 feet which could be critical in any actual mission performed by a vehicle 
equipped with this system as its only navigation source. The rest of the graphs as shown 
in Figure 18 on page 35, Figure 19 on page 36, and Figure 20 on page 37 show the fil- 
ter's ability to track any deviations in the Set and Drift experienced by the vehicle with 
only one sample after the change. 
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Figure 17. Intended and Actual Vehicle Tracks With Multiple Set and Drift 
Changes 
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Figure 18. Position Coordinates With Multiple Set and Drift Changes 



35 




* t • • « • • 

o eo r- 10 ui cn n r- o 



S1CQA 



C i J*t 3 3 3 

o S t/> r*j o S u"> fvj o 



o qd U 3 ui cn <m ca 




sioax 

E 3 3 </) i us 5 3 3 

o K lD rj O S Lf) Xu o 

0®f^ CO if} m (V* -r- o 






Figure 19. Components With Multiple Set and Drift Changes 
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Tigure 20. Set and Drift Components With Multiple Set and Drift Changes 
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E. STATE ESTIMATION WITH MEASUREMENT NOISE 

In this simulation noise was added to the measurements of .v, y, and ^ in order to 
determine how the Kalman Filter would respond. This noise was added using the 
Normal function described in the previous section. A different function was generated 
for each measurement with unique seed numbers, in order to preserve their independ- 
ence, and zero mean about the actual value. The resulting graphs are displayed in 
Figure 2! on page 40, Figure 22 on page 41, Figure 23 on page 42, and Figure 24 on 
page 43. The run time was 4200 seconds and as in the previous simulation the Set and 
Drift were changed every other sample. 

The graphs show the same results which were achieved in the third simulation with 
some small differences. The vehicle's intended and actual tracks are much closer than 
before which is due to a lesser amount of deviation in the Set and Drift variables. A 
close look at the plots of the state variables show some variation between their estimated 
and actual values which was not present before. These are the errors produced by the 
measurement noise. In order to see the actual differences, three tables were generated 
which show the actual and estimated values against time. The position coordinates, X 
and Y, are shown in Table 2 on page 39, .v and y are in Table 3 on page 39. and 
ux and uy are in Table 4 on page 39. 

From the tables, the errors range from 3 feet up to 21 feet for x andy, are less than 
.02 feet per second for .t andy, and vary’ up to .IS feet per second for ux and uy. Note 
also that during the periods when there is no change in the Set and Drift, the estimates 
show a significant improvement. Errors such as these may be tolerable depending on the 
vehicle's assigned mission. The best way to minimize the error would be to take multiple 
GPS readings in order to obtain a better fix. But this would increase the amount of time 
that the vehicle would spend on the surface. 
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Table 2. DIFFERENCES IN POSITION COORDINATES 



TIME 


X 


Y 




ACTUAL 


ESTIMATED 


ACTUAL 


ESTIMATED 


600. 


5092.4 


5098.4 


4103.5 


4131.1 


1200. 


10056. 


10060. 


8351.7 


8368.0 


1800. 


14SS0. 


14890. 


12712. 


12708. 


2400. 


19803. 


19801. 


16962. 


16971. 


3000. 


24869. 


24863. 


21097. 


21103. 


3600. 


29822. 


29831. 


25359. 


25367. 


4200. 


345 IS. 


34525. 


29S64. 


29870. 



Table 3. DIFFERENCES IN VELOCITY COMPONENTS 



TIME 


.Y 


Y 




ACTUAL 


ESTIMATED 


ACTUAL 


ESTIMATED 


600. 


8.4862 


8.5048 


6.S423 


6.8961 


1200. 


8.0398 


8.0520 


7.2664 


7.23 18 


1 800. 


8.0398 


8.0520 


7.2666 


7.231S 


2400. 


8.4441 


S. 1857 


6.8931 


7. 1053 


3000. 


8.4441 


S.433S 


6.S931 


6.SS7S 


3600. 


7.8266 


8.2801 


7.508 1 


7.1072 


4200. 


7.S266 


7.8265 


7.5081 


7.5015 



Table 4. DIFFERENCES IN SET AND DRIFT COMPONENTS 



TIME 


ux 


UY 




ACTUAL 


ESTIMATED 


ACTUAL 


ESTIMATED 


600. 


-.92089 


-.90220 


-1.3892 


-1.3354 


1200. 


-1.1535 


-1.1414 


-1.2030 


-1.4073 


1 800. 


-1.1535 


-1.1414 


-1.2030 


-1.2378 


2400. 


-.91472 


-1.1730 


-1.3932 


- 1 . IS 1 1 


3000. 


-.91472 


-.92496 


-1.3932 


-1.39S5 


3600. 


-1.3426 


-.88910 


-.98751 


-1.3884 


4200. 


-1.3426 


-1.3428 


-.9S751 


-.99409 
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Figure 21. Intended and Actual Vehicle Tracks With Measurement Noise 
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Figure 22. Position Coordinates With Measurement Noise 
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Figure 23. Velocity Components With Measurement Noise 
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Figure 2*4, Set and Drift Components With Measurement Noise 
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V. CONCLUSION 



A. ANALYSIS 

The simulation results from Chapter 4 demonstrated the Extended Kalman Filter's 
ability to successfully identify the total Set and Drift that an underwater vehicle could 
experience on its journey. However, there are some limitations which have appeared. 
First, if the magnitude of the Set and Drift vary extensively between samples, then the 
vehicle's ability to navigate precise tracks is severely limited. There are three major 
factors which affect how far the vehicle will be driven off its intended course: the mag- 
nitude of the Set and Drift, the vehicle's ordered speed, and the time between samples. 
There is no control over the first factor but there is some latitude over which the oper- 
ator can control. Increasing the speed and decreasing the sampling interval will allow 
less deviation from an intended track. However, both the resulting increase in power 
consumption and the increase in the probability of detection will have to be taken into 
account. 

Another major limitation with using this system as a vehicle's sole navigation source 
is that the covert aspect of an underwater vehicle becomes lost awhen the GPS fixes are 
taken. Because of the frequencies at which GPS operates have very little ability to travel 
through water, the vehicle's antenna must break through the water's surface. To mini- 
mize the risk of detection, a system would have to be designed which would allow the 
antenna to be raised and lowered from a safe depth. An alternative would be to use the 
Omega Navigation System, whose frequencies can be reach down to about 40 feet, as a 
prime source and use GPS as an update at regular intervals or whenever the Omega data 
is suspect. This technique could allow the vehicle to refrain from surfacing as much and 
at a rate comparable to that required in updating an I MU system. 

B. APPLICATIONS 

1. Back up to an IMU 

The most obvious place for this design is as a backup to an IMU system 
onboard an underwater vehicle which would take over in event of a failure. Current 
design philosophy seems to be centered on using conventional dead reckoning systems 
in which the angles and components are calculated using trigonometric functions. These 
type of complex calculations are difficult for a microprocessor to accomplish and ma- 
chine roundoff errors can occur if the angles are close together. This Extended Kalman 
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Filter design requires only one trigonometric function [Eqt. 3.15] and the matrix 
calculations are ideally suited for a microprocessor. 

2. Mine Location 

Because this system is lightweight, inexpensive, and has very' little material 
which will disturb a magnetic field, a large fleet of plastic mine locating escorts could 
be developed. These escorts could lead ships through a mined area by locating suspected 
mine positions. As such, the vehicle would be composed of a body, a propulsion unit, 
a simple attitude control section, and a detector. This detector would be the most ex- 
pensive component and it could take the form of a high resolution sonar or a magnetic 
anomaly detector. The shape of the vehicle could be designed that would reduce any 
pressure waves which would set off any pressure sensitive mines. The antenna system 
would not have to be as complicated as mentioned previously because the need to op- 
erate covertly would be erased. The communication link could be liber optic and go to 
a mother ship or a group of mother ships working together. 

3. Long Range Reconnaisance 

A variant of the mine locating vehicle discussed above, would be as a long range 
reconnaisance vehicle with the mine detector replaced with some other sensor such as a 
communications receiver or an infrared detector. The missions would be limited to those 
which do not require precise navigation such as coast or ship surveillance. The advan- 
tages to using this system is that the power needed to run an I MU would be eliminated 
and and the costs significantly reduced. Communication could be in the form of fiber 
optics for realtime information. Radio or data storage systems could be used for non- 
realtime. Delivery and retrieval could be accomplished through the use of a submarine, 
a ship, or small boats. 

C. RECOMMENDATIONS FOR FUTURE RESEARCH 

The following areas present valid opportunities for useful expansion of this work: 

• Develop an antenna and support system, in order to receive the GPS signals in the 
presence of low to medium sea state and allow the vehicle to remain at depth. 

• Build a prototype Extended Kalman Filter and test it as a back up on an existing 
autonomous underwater vehicle. 
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APPENDIX A. SIMULATION PROGRAM 



This program simulates an underwater vehicle traveling through the water and is 
written in DSL. The Dynamic section mimics the vehicle's true dynamics, which were 
modeled in Chapter 2 as a second order system, and thereby calculates the vehicles true 
position and heading angle. The Sample section samples the system's position and 
heading angle every T seconds and these measurements are sent to the subroutine 
Kalman which estimates the states. After the estimates are returned, the program cal- 
culates a new heading command and updates the predicted state vector XP. 



TITLE VEHICLE SIM WITH KALMAN CORRECTION TO DEST / VAR F AND BIAS 

PARAM VEL=7. 5,FVEL=1. ,FANG=45. , N=10,N1=7 ,N2=13 ,N3=3 

PARAM AX=0. , AY=0 . , BX=14. 0 , BY=12. 0 

PARAM SIGCA=0. 3,SIGCV=0. 3 , SIGX2=100. , SIGY2=100. ,SGYAW2=. 1 ,... 

SIGCY=. 1 ,SIGCX=. 1 ,SIGX=10. ,SIGY=10. ,SIGYAW=. 01 
D DIMENSION WAPNTX( 0: 10) ,WAPNTY( 0: 10 ) 

D DIMENSION F(6,6) , XHAT(6) ,XP(6) , Y( 3) ,Q( 2 , 2) ,R( 3 , 3) ,P(6 ,6) ,H( 3) , 

D 1 HP( 3 , 6 ) ,G(6,2) ,XM( 6) 

EXCLUDE IER,F,XHAT,XP,Y,Q,R,P,H,HP,G,XM 
D DATA F,XHAT,HP,Y,Q,R,P,H,XP,G / 133 *0. / 

FIXED I,J,N,N1,N2,N3 
INITIAL 

Al=2. 

Tl=5. 

A=0. 

TIMEW=0. 

T=DELS 

Vr 

* ADDITION OF ERROR TO PREDICTED CURRENT VELOCITY AND ANGLE 
FANGS=(FANG+180. )*DEGRA + .2 
FVELS=FVEL 

FVELS=(FVELS*6000. )/3600. 

FVEL=(FVEL*6000. )/3600. 

VEL=(VEL*6000. )/3600. 

BX=BX*6000. 

BY=BY*6000 

UXDOTS=FVELS*COS ( FANGS ) 

UYDOTS=FVELS*SIN( FANGS) 

DC=ATAN2( BY-AY, BX-AX) 

DIST=SQRT( ( BX-AX)**2t( BY-AY)**2) 

INT=DIST/10. 

DO 10 1=0,10 

WAPNTX( I)=I * I NT 1 ’' COS (DC) 

WAPNTY( I)=I * INT*SIN( DC) 

WX=WAPNTX( I) 

WY=WAPNTY( I) 



46 



CALL SAVE( S3) 

10 CONTINUE 

INITIALIZE KALMAN MATRICIES 

* INITIALIZE PHI 

if 

DO 11 1=1,6 
F(I,I)=1. 

11 CONTINUE 
F ( 1 , 2 ) =T 
F ( 3 , 4 ) =T 

* 

* INITIALIZE Q 

* 

Q( 1 , 1)=SIGCX 
Q( 2 , 2)=SIGCY 

* 

* INITIALIZE GAMMA 1 

* 

G(1,1)=T 

G(2,l)=l. 

G( 3 , 2)=T 
G( 4, 2)=1. 

G(5,l)=l. 

G( 6 , 2)=1. 

* 

INITIALIZE R 

if 

R( 1 , 1)=SIGX2 
R( 2 , 2)=SIGY2 
R( 3 , 3)=SGYAW2 

* 

* INITIALIZE XHAT WITH PREDICTED VALUESOF CURRENTCOMPONENTS 

if 

X HAT(5)=0. 

XHAT( 6)=0. 

* XHAT(5)=FVEL * COS( (FANG+180. )*DEGRA) 

* XHAT( 6 ) =FVE L * S IN( ( FANG+ 180.) *DEGRA ) 

if 

* INITIALIZE HP 

if 

HP(1,1)=1. 

HP( 2 , 3)=1. 



* DETERMINATION OF ACTUAL X AND Y COORDINATES( SUNS INPUT) 

if 

DERIVATIVE 

HDGERR=YAW - HDG 
K=-4. 4l*HDGERR 
YAWDOT=REALPL( 0. 25, K) 

YAW=INTGRL( 0 . , YAWDOT ) 

XDOT=VEL*COS( YAW) 

XDOTS=XDOT+UXDOTS 
YDOT=S I N( Y AW ) *VEL 
YDOTS=UYDOTS+YDOT 
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Y1=INTGRL( 0. , YDOTS ) 

X=INTGRL( 0. ,XDOTS) 

SAMPLE 

IF(TIME. EQ. 0. )THEN 
FANG=FANG * DEGRA 
IF(SIN(FANG)*SIN(DC). GT. 0)THEN 
ALPHA=PI -ABS(FANG)+DC 
BETA=AS IN( FVEL-’-S IN( ALPHA) / VEL) 

HDG=DC + BETA 
ELSE 

ALPHA=PI -ABS(FANG) -DC 
BETA=ASIN( FVEL*S IN( ALPHA) /VEL) 

HDG=DC - BETA 
END IF 

* HDG=DC 

XHAT( 2 )=VEL*COS ( HDG ) 

XHAT(4)=VEL*SIN(HDG) 

XP(2)=VEL*C0S(HDG) 

XP( 4)=VEL*SIN( HDG) 

YO-;JDG 
DO 25 1=1,6 
XM( I )=XP( I ) 

25 CONTINUE 

CALL KALMAN( A , XHAT ,F,XP,Y,Q,R,P,H,HP,G,IER) 
ELSE 

A=A + 1. 

* DETERMINATION OF REQUIRED COURSE TO DESTINATION 

30 DWY=WAPNTY( 10) - Y1 

DWX=W APNTX (10)- X 
DC=ATAN 2 ( DWY , DWX ) 

* CALCULATE FORCE COMPONENTS , UANG AND UVEL 

UPDATE H, Z, AND HP MATRICIES 

H( 1)=XP( 1) 

H( 2)=XP( 3) 

H( 3)=ATAN2( XP( 4) -X?( 6) ,XP( 2) -XP( 5 ) ) 

DEN=( XP( 2) -XP( 5 ) )**2 + (XP(4) -XP( 6) )**2 

HP(3,2)=(XP(6)-XP(4))/DEN 

HP( 3 , 4)=(XP( 2) -XP( 5) ) /DEN 

HP( 3 , 5 )=-HP( 3,2) 

HP( 3 , 6)=-HP( 3,4) 

Y ( 1 ) =X + NORMAL( N1 , 0 , SIGX) 

Y(2)=Y1 + NORMAL( N2 , 0 , SIGY) 

Y( 3 )=YAW+ NORMALC N3 , 0 , SIGYAV) 

DO 35 1=1,6 
XM( I )=XP( I ) 

35 CONTINUE 



* KALMAN FILTER UPDATE OF XHAT 

/V 

CALL KALMAN( A , XHAT , F , XP , Y , Q , R , P , H , HP , G , IER) 

* 

* CALCULATION OF ESTIMATED CURRENT VELOCITY AND ANGLE FROM UPDATED XHAT 

* 

UVEL=SQRT( XHAT( 5 ) **2+XHAT( 6 ) **2 ) 
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UANG=ATAN( XHAT( 6 ) /XHAT( 5 ) ) 



* CALCULATION OF STEERED COURSES TO WAYPOINT AND DESTINATION 

IF( A. EQ. Al-1. )THEN 
IF(SIN(UANG)*SIN(HDG). GT. 0)THEN 
ALPHA=PI -ABS(UANG) +DC 
BETA=ASIN(UVEL*SIN( ALPHA) /VEL) 

HDGD=DC + BETA 
ELSE 

ALPHA=P I - AB S ( UANG ) - DC 
BETA=AS IN ( UVEL*S IN( ALPHA) /VEL) 

HDGD=DC - BETA 
END IF 

* 

* UPDATE X( K+l/K) FOR CRSE CHANGE 

* 

XP(2)= VEL * COS(HDGD) + XHAT(5) 

XP(1)= T * XP( 2) + XHAT(l) 

XP( 4)= VEL * SIN( HDGD)+ XHAT(6) 

XP( 3 )= T * XP( 4) + XHAT( 3) 

HDG=HDGD 
ELSE 
HDG=HDG 
END IF 
ENDIF 

DYNAMIC 

IF( A. EQ. A1)THEN 

BIAS=NORMAL(N, 0. ,.3) 

UXDOTS=FVELS " COS ( F ANGS+B IAS) 
UYDOTS=FVELS*SIN(FANGS+BIAS) 

Al=Al+2. 

T1=TIME + 2. 

ELSE 

UXDOTS=UXDOTS 

UYDOTS=UYDOTS 

ENDIF 

IF(TIME.LT. T1)CALL SAVE(S2) 

X1KK=XHAT( 1) 

X2KK=XHAT( 2) 

X3KK=X'HAT( 3) 

X4KK=XHAT( 4) 

X5KK=XHAT(5) 

X6KK=XHAT( 6) 

X1KKM1=XM( 1) 

X2KKM1=XM(2) 

X3KKM1=XM( 3) 

X4KKM1=XM( 4) 

X5KKM1=XM(5) 

X6KKM1=XM( 6) 

40 IF(X. GE.BX) CALL ENDRUN 

CONTROL FINTIM=9000. ,DELS=600. 

METHOD ADAMS 

SAVE (SI) 600. ,X1KK,X2KK,X3KK,X4KK,X5KK,X6KK, . . . 

X 1KKM 1 , X2KKM 1 , X3 KKM 1 , X4KKM 1 , X5 KKM 1 , X6KKM 1 
( S 2 ) 600. , X , Y1 , UXDOTS , UYDOTS , XDOTS , YDOTS 



SAVE 



SAVE ( S3 )4000. ,WX,WY 

PRINT 600 ,HDG ,X , Y1 , UXDOTS . UYDOTS 

GRAPH (G1/S2 ,DE=TEK618)X( SC=8. 4E3 ) , Yl( SC=10. 5E3 ,LO=0. ) 

GRAPH (G2/S3 ,DE=TEK618 , OV)WX( SC=8. 4E3 , AX=OHIT) , . . . 

WY(MA=4,SC=10. 5E3 , AX=OMIT) 

GRAPH ( G3/S2 , DE=TEK6 18 )TIME , X( LI=1 , AX=OMIT , L0=0 ) 

GRAPH ( G4/S 1 , DE=TEK6 18 , OV)TIME , X1KK( LI=2 , L0=0 ) , X1KKM1( LI=3 , L0=0 ) 

GRAPH ( G5 /S2 , DE=TEK6 18 )TIME , Yl( LI=1 , AX=OMIT, L0=0 ) 

GRAPH ( G6/S 1 , DE=TEK6 18 , OV)TIME , X3KK( LI=2 , L0=0 ) , X3KKM1( LI=3 , L0=0 ) 

GRAPH (G7/S2,DE=TEK618)TIME ,XDOTS( AX=OMIT,LI=l , L0=0. ,SC=1. 8) 

GRAPH (G8/S 1 , DE=TEK618 , OV)TIME ,X2KK( LI=2 , L0=0 , SC=1. 8),. . . 

X2KKM1( LI=3 , L0=0. ,SC=1. 8) 

GRAPH ( G9/S2 , DE=TEK6 18 )TIME , YDOTS( AX=OMIT , LI=1 , LO=- 1. 5 , SC=1. 5 ) 

GRAPH (G10/S1,DE=TEK618 , OV)TIME ,X4KK( LI=2, L0=- 1. 5 , SC=1. 5) . 

X4KKM1( LI=3 , LO=- 1. 5 , SC=1. 5) 

GRAPH (G11/S2 ,DE=TEK618)TIME , UXDOTS (AX=OMIT , LI=1 , LO=-2. 0 , SC=. 5) 

GRAPH (G12/S1 ,DE=TEK618 ,0V)TIME ,X5KK( LI=2 , L0=-2. 0,SC=. 5),. . . 

X5KKM1( LI=3 , LO=-2. 0,SC=. 5) 

GRAPH (G13/S2 ,DE=TEK618)TIME , UYDOTS (AX=OM IT, L 1=1 , LO=-2. 0 , SC=. 5 ) 

GRAPH ( G14/S1 ,DE=TEK618 ,OV)TIME ,X6KK(LI=2 , LO=-2. 0 , SC=. 5),. . . 

X6KKM1( LI=3 , LO=-2. 0,SC=. 5) 

LABEL (ALL) KALMAN CORRECTION TO DEST /BIAS= VAR/CUR CHG=2/MSMT ERROR/ 

END 

STOP 
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APPENDIX B. KALMAN FILTER PROGRAM 



This program is written in Fortran 77 and performs the necessary matrix calcu- 
lations for the extended Kalman Filter developed in Chapter 3. The routine calls three 
IMSLDP subroutines, LEQT1F, VMULFF, and VMULFP which accomplish the ma- 
trix multiplication operations. In order for Kalman to be compatible with DSL. it was 
written in double precision. After performing the calculations, this subroutine returns 



the predicted covariance matrix (P), the updated state estimate vector (XHAT), and the 



predicted state vector (XP) to the calling program. 
C 



C 



C 



C 

C 

C 15 



SUBROUTINE KALMAN ( A ,X, PHI ,XKKM1 ,Z ,Q ,R ,P , HK,HKP , GAMMA , IER) 
SPECIFICATIONS FOR ARGUMENTS 
IMPLICIT RE AL*8 (A-H,0-Z) 



INTEGER 

DIMENSION 



1 

2 



INTEGER 
IER = 0 
N=6 
NY=3 
NX=1 



IER 

X(6),XKKM1(6) ,PHI(6,6),Z(3),Q(2,2) ,R(3,3), 
HK( 3) ,HKP( 3 ,6),T1(6,6),T2(6,6),T3(6),P(6,6), 
GAMMAC 6 , 2) ,G( 6 , 3) 

I , J , N , NX , NY , L 



L=2 

K=A 

WRITE ( 6 , 2) ( (HKP( I , J) , J=1 , 6) , 1=1 , 3) 

WRITEC 6 ,5) (XKKM1( J) , J=l,6) 

WRITE(6,6)(HK(J), J=l,3) 

WRITE ( 6 , 12) ( Z( J) , J=1 ,3) 

CALCULATE P IF K = ZERO 
IF (A . EQ. 0) THEN 
WRITE(6,3)((PHI(I,J) , J=1 ,6) , 1=1 ,6) 

WRITE( 6,7)((Q(I,J),J=1,2),I=1,2) 

WRITEC 6, 8) ((GAMMAC I, J), J=l,2), 1=1,6) 

CALL VMULFF ( GAMMA, Q, N, L, L,N,L,T2,N, IER) 
CALL VMULFP (T2, GAMMA, N, L, N,N,N, P,N,IER) 
DO 15 1=1,6 
P( I , I )=4000. 

CONTINUE 

WRITEC 6 , 13) ( (P( I , J) , J=1 , 6) , 1=1 ,6) 

ELSE 

CONTINUE 
END IF 



C CALCULATE GAIN MATRIX G(K) 

CALL VMULFP (HKP , P,NY, N, N,NY,N,T2,N, IER) 
CALL VMULFP (T2,HKP,NY, N,NY,N,NY,T1,N, IER) 
DO 35 I = 1 ,NY 
DO 30 J = 1 ,NY 

Tl( I , J) = Tl( I , J) + R( J, I) 

30 CONTINUE 
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35 



40 



45 

50 



C 



70 

75 



C 



60 



65 



C 



10 



20 

25 



C 



80 

9000 

9005 

1 

2 

3 

4 

5 

6 



CONTINUE 

CALL LEQT1F(T1,N,NY,N,T2,0,T3,IER) 

IF ( IER . EQ. 0) GO TO 40 
IER = 130 
GO TO 9000 
DO 50 I = 1 ,NY 
DO 45 J = 1,N 

G( J, I ) = T2(I,J) 

CONTINUE 

CONTINUE 

WRITE( 6 ,9)((G(I,J),J=1,3),I=1,6) 

CALCULATE P(K/K) 

CALL VMULFF ( G , HKP , N , NY , N,N,NY,T2,N, IER) 
CALL VMULFF (T2, P, N, N, N.N.N.Tl ,N, IER) 
DO 75 I = 1 ,N 
DO 70 J = 1 ,N 

P(I.J) = P(I,J) - Tl(I.J) 

CONTINUE 

CONTINUE 

WRITE ( 6,11)((P(I,J), J=1 ,6) , 1-1,6) 
CALCULATE X(K/K) 



DO 60 I = 1 ,NY 

T3( I ) = Z(I) - HK(I) 

CONTINUE 

CALL VMULFF (G,T3, N,NY,NX,N,NY,T2,N, IER) 

DO 65 I = 1,N 

X(I) = XKKMl(I) + T2( 1,1) 

CONTINUE 

WRITE ( 6 , 4) (X( J) , J=1 ,6) 

CALCULATE P(K+1/K) 

CALL VMULFF ( PHI, P, N, N, N ,N , N ,T1 ,N , IER) 
CALL VMULFP (Tl, PHI, N, N, N , N , N , P,N,IER) 
CALL VMULFF ( GAMMA, Q, N, L, L,N,L,T2,N, IER) 
CALL VMULFP (T2, GAMMA, N, L, N,N,N,T1 ,N, IER) 
DO 25 I = 1 ,N 
DO 20 J = 1,N 

P(I,J) = P(I,J) + Tl( I , J) 

P(I,J) = P(I,J) 

CONTINUE 

CONTINUE 

WRITE( 6 , 1) ( ( P( I , J) , J=1 ,6) , 1=1,6) 

CALCULATE X(K+1/K)=XKKM1 
CALL VMULFF ( PHI, X, N, N,NX,N,N,T1 ,N, IER) 

DO 80 I = 1 ,N 

XKKMl(I) = Tl( 1,1) 

CONTINUE 
GO TO 9005 
CONTINUE 

CALL UERTST ( IER , 6HFTKALM) 

RETURN 

PKP 1K= ' ,/,6(3X,F12. 4, 3X) ) 

ZP=' ,/,6(3X,F12. 4,3X)) 

PHI= ' ,/,6(3X,F12. 4, 3X) ) 

XKK=' ,/ ,6( 3X,F12. 4,3X)) 

XKKM 1= ' ,/,6(3X,F12. 4, 3X) ) 

HK=' ,/,3(3X,F12. 4 , 3X) ) 



FORMAT ( / , ' 
FORMAT ( / , ' 
FORMAT ( / , ' 
FORM AT (/ , ' 
FORMAT ( / , ' 
FORMAT! / , ' 



7 FORMAT ( / , ’ 

8 FORMAT ( / , ' 

9 FORMAT ( / , ’ 

11 FORMAT (/,' 

12 FORMAT ( / , ' 

13 FORMAT ( / , ’ 
END 



Q= >/ >2(3X,F12.4,3X)) 
GAMMA= ' ,/,2(3X,F12. 4,3X)) 
KALMAN GAINS =' ,/ , 3( 3X,F12. 
PKK=' ,/,6(3X,F12. 4,3X)) 

Z=' , / , 3( 3X,F12. 4 , 3X) ) 

P/K=0 =’ ,/,6(3X,F12. 4,3X)) 



> 3X) ) 
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